#pragma once

#include <compl.h>

typedef word_t					bitmap_t;

// SASSERT(is_powerof2(sizeof(bitmap_t)));

/* 避免除法和取余 */
#define BITS_PERSIZE				(sizeof(bitmap_t) * BYTE_WIDTH)
// #define BITS_LOG2				(__CLZL(BITS_PERSIZE))
#define BITS_TO_WORDS(nbits)			((nbits) / BITS_PERSIZE)

#define BITS_TO_BYTELEN(nbits)			(BITS_TO_WORDS(nbits) * sizeof(bitmap_t))

#define BITIDX_TO_WORDIDX(bitidx)		((bitidx) / BITS_PERSIZE)
#define BITIDX_TO_WORDBIDX(bitidx)		((bitidx) % BITS_PERSIZE)

#define SMALL_CONST_NBITS(nbits)		(nbits <= WORD_WIDTH)

SAI void bitmap_zero(bitmap_t *dst, size_t nbits){
	if (SMALL_CONST_NBITS(nbits)){
		dst[0] = 0;
	}
	else{
		__memset(dst, 0, BITS_TO_BYTELEN(nbits));
	}
}

SAI void bitmap_fill(bitmap_t *dst, size_t nbits){
	if (SMALL_CONST_NBITS(nbits)){
		dst[0] = ((bitmap_t)0 - 1);
	}
	else{
		__memset(dst, 0xff, BITS_TO_BYTELEN(nbits));
	}
}

SAI void bitmap_neg(bitmap_t *dst, size_t nbits){
	size_t idx = 0;
	for(idx = 0; idx < BITS_TO_WORDS(nbits); idx++){
		dst[idx] = ~dst[idx];
	}
}

SAI void bitmap_rshift(bitmap_t *dst, size_t nbits, size_t rshift_bit){
	size_t idx;
	// ASSERT(rshift_bit <= nbits);
	/* 计算idx偏移量 */
	size_t idx_offst = BITIDX_TO_WORDIDX(rshift_bit % nbits);
	size_t offst = BITIDX_TO_WORDBIDX(rshift_bit % nbits);
	bitmap_t src_neighbour = 0;
	bitmap_t src;
	for(idx = 0; idx < BITS_TO_WORDS(nbits); idx++){
		/* 源，如果出界则为空 */
		src = ((idx + idx_offst) < BITS_TO_WORDS(nbits)) ?  dst[idx + idx_offst] : 0;
		/* 源的后一个邻居 */
		src_neighbour = (idx + idx_offst + 1 < BITS_TO_WORDS(nbits)) ? dst[idx + idx_offst + 1] : 0;
		/* 拼接 */
		dst[idx] = (src >> offst) | (src_neighbour << (BITS_PERSIZE - offst));
	}
}

SAI void bitmap_lshift(bitmap_t *dst, size_t nbits, size_t lshift_bit){
	size_t idx = BITS_TO_WORDS(nbits);
	// ASSERT(lshift_bit <= nbits);
	/* 计算idx偏移量 */
	size_t idx_offst = BITIDX_TO_WORDIDX(lshift_bit % nbits);
	size_t offst = BITIDX_TO_WORDBIDX(lshift_bit % nbits);
	/* 从最后开始左移 */
	bitmap_t src_neighbour = 0;
	bitmap_t src;

	do{
		idx -= 1;
		/* 源，如果出界则为空 */
		src = (idx >= idx_offst) ?  dst[idx - idx_offst] : 0;
		/* 源的后一个邻居 */
		src_neighbour = (idx >= idx_offst + 1) ? dst[idx - idx_offst - 1] : 0;
		/* 拼接 */
		dst[idx] = (src << offst) | (src_neighbour >> (BITS_PERSIZE - offst));
	} while (idx != 0);
}

/* bitmap 找到第一个设置的位数 */
SAI size_t bitmap_ffs(bitmap_t *dst, size_t nbits){
	size_t idx = 0;
	size_t ret_bitidx = 0;
	size_t set_bitidx;
	for(idx = 0; idx < BITS_TO_WORDS(nbits); idx++){
		set_bitidx = ffs_generic(dst[idx]);
		if (set_bitidx == 0){
			ret_bitidx += BITS_PERSIZE;
		}
		else{
			ret_bitidx += set_bitidx;
			return ret_bitidx;
		}
	}
	return 0;
}

/* 计算bitmap中所有1的个数 */
SAI size_t bitmap_popcount(bitmap_t *dst, size_t nbits){
	size_t idx = 0;
	size_t ret_bitnr = 0;
	for(idx = 0; idx < BITS_TO_WORDS(nbits); idx++){
		ret_bitnr += popcount_generic(dst[idx]);
	}
	return ret_bitnr;
}

SAI void bitmap_clr(bitmap_t *dst, size_t bit_idx){
	size_t idx = BITIDX_TO_WORDIDX(bit_idx);
	size_t bidx = BITIDX_TO_WORDBIDX(bit_idx);
	dst[idx] &= ~BIT(bidx);
}

SAI void bitmap_set(bitmap_t *dst, size_t bit_idx){
	size_t idx = BITIDX_TO_WORDIDX(bit_idx);
	size_t bidx = BITIDX_TO_WORDBIDX(bit_idx);
	dst[idx] |= BIT(bidx);
}

/* 多核clr/set */
SAI void bitmap_mclr(bitmap_t *dst, size_t bit_idx){
	size_t idx = BITIDX_TO_WORDIDX(bit_idx);
	size_t bidx = BITIDX_TO_WORDBIDX(bit_idx);
	// dst[idx] &= ~BIT(bidx);
	atm_bitops(and, &dst[idx], ~BIT(bidx));
}

SAI void bitmap_mset(bitmap_t *dst, size_t bit_idx){
	size_t idx = BITIDX_TO_WORDIDX(bit_idx);
	size_t bidx = BITIDX_TO_WORDBIDX(bit_idx);
	// dst[idx] |= BIT(bidx);
	// atm_bitops(or, &dst[idx], BIT(bidx));
	/* 假设是0 可以直接用faa */
	faa(&dst[idx], BIT(bidx));
}

SAI bool_t bitmap_isset(bitmap_t *dst, size_t bit_idx){
	size_t idx = BITIDX_TO_WORDIDX(bit_idx);
	size_t bidx = BITIDX_TO_WORDBIDX(bit_idx);
	return (bool_t)(!!(dst[idx] & BIT(bidx)));
}

SAI void bitmap_copy(bitmap_t *dst, const bitmap_t *src, size_t nbits){
	if (SMALL_CONST_NBITS(nbits)){
		dst[0] = src[0];
	}
	else{
		__memcpy(dst, src, BITS_TO_BYTELEN(nbits));
	}
}

SAI void bitmap_and(bitmap_t *bm0, bitmap_t *bm1, bitmap_t *bm2, size_t nbits){
	size_t idx = 0;
	for(idx = 0; idx < BITS_TO_WORDS(nbits); idx++){
		bm0[idx] = bm1[idx] & bm2[idx];
	}
}

SAI void bitmap_or(bitmap_t *bm0, bitmap_t *bm1, bitmap_t *bm2, size_t nbits){
	size_t idx = 0;
	for(idx = 0; idx < BITS_TO_WORDS(nbits); idx++){
		bm0[idx] = bm1[idx] | bm2[idx];
	}
}

SAI void bitmap_xor(bitmap_t *bm0, bitmap_t *bm1, bitmap_t *bm2, size_t nbits){
	size_t idx = 0;
	for(idx = 0; idx < BITS_TO_WORDS(nbits); idx++){
		bm0[idx] = bm1[idx] ^ bm2[idx];
	}
}



/* 静态bitmap, bits需要对齐 */
#define DEF_SBITMAP(name, bits)				SASSERT(bits/sizeof(bitmap_t) * sizeof(bitmap_t) == bits); bitmap_t name[BITS_TO_WORDS(bits)];
#define SBITMAP_ERROR						UNREACHABLE()

#define SBITMAP_FILL_S(pbitmap, nbits)		(nbits <= sizeof_obj(pbitmap) * BYTE_WIDTH ? bitmap_fill(pbitmap, nbits) : SBITMAP_ERROR)
#define SBITMAP_ZERO_S(pbitmap, nbits)		(nbits <= sizeof_obj(pbitmap) * BYTE_WIDTH ? bitmap_zero(pbitmap, nbits) : SBITMAP_ERROR)
#define SBITMAP_NEG_S(pbitmap, nbits)		(nbits <= sizeof_obj(pbitmap) * BYTE_WIDTH ? bitmap_neg(pbitmap, nbits) : SBITMAP_ERROR)
#define SBITMAP_FFS_S(pbitmap, nbits)		(nbits <= sizeof_obj(pbitmap) * BYTE_WIDTH ? bitmap_ffs(pbitmap, nbits) : 0)
#define SBITMAP_POPCOUNT_S(pbitmap, nbits)	(nbits <= sizeof_obj(pbitmap) * BYTE_WIDTH ? bitmap_popcount(pbitmap, nbits) : 0)

#define SBITMAP_CLR_S(pbitmap, bidx)		(bidx < sizeof_obj(pbitmap) * BYTE_WIDTH ? bitmap_clr(pbitmap, bidx) : SBITMAP_ERROR)
#define SBITMAP_SET_S(pbitmap, bidx)		(bidx < sizeof_obj(pbitmap) * BYTE_WIDTH ? bitmap_set(pbitmap, bidx) : SBITMAP_ERROR)
#define SBITMAP_ISSET_S(pbitmap, bidx)		(bidx < sizeof_obj(pbitmap) * BYTE_WIDTH ? bitmap_isset(pbitmap, bidx) : FALSE)

#define SBITMAP_COPY_S(pbmap0, pbmap1, pbmap2, nbits)		((sizeof_obj(pbmap0) == sizeof_obj(pbmap1) && 	\
															sizeof_obj(pbmap1) == sizeof_obj(pbmap2) && 	\
															nbits <= sizeof_obj(pbmap0) * BYTE_WIDTH) ? 	\
															bitmap_copy(pbmap0, pbmap1, pbmap2, nbits) : SBITMAP_ERROR)

#define SBITMAP_AND_S(pbmap0, pbmap1, pbmap2, nbits)		((sizeof_obj(pbmap0) == sizeof_obj(pbmap1) && 	\
															sizeof_obj(pbmap1) == sizeof_obj(pbmap2) && 	\
															nbits <= sizeof_obj(pbmap0) * BYTE_WIDTH) ? 	\
															bitmap_and(pbmap0, pbmap1, pbmap2, nbits) : SBITMAP_ERROR)

#define SBITMAP_OR_S(pbmap0, pbmap1, pbmap2, nbits)			((sizeof_obj(pbmap0) == sizeof_obj(pbmap1) && 	\
															sizeof_obj(pbmap1) == sizeof_obj(pbmap2) && 	\
															nbits <= sizeof_obj(pbmap0) * BYTE_WIDTH) ? 	\
															bitmap_or(pbmap0, pbmap1, pbmap2, nbits) : SBITMAP_ERROR)

#define SBITMAP_XOR_S(pbmap0, pbmap1, pbmap2, nbits)		((sizeof_obj(pbmap0) == sizeof_obj(pbmap1) && 	\
															sizeof_obj(pbmap1) == sizeof_obj(pbmap2) && 	\
															nbits <= sizeof_obj(pbmap0) * BYTE_WIDTH) ? 	\
															bitmap_xor(pbmap0, pbmap1, pbmap2, nbits) : SBITMAP_ERROR)



// #define BIT_WORD(nr)		((nr) / WORD_WIDTH)

// #define BITMAP_FIRST_WORD_MASK(start) (~0UL << ((start) & (WORD_WIDTH - 1)))
// #define BITMAP_LAST_WORD_MASK(bit_idx) (~0UL >> (-(nbits) & (WORD_WIDTH - 1)))

// #define DEF_BITMAP(name,bits) \
//     size_t name[BITS_TO_LONGS(bits)];

// #define SMALL_CONST_NBITS(nbits)    nbits <= WORD_WIDTH

// SAI void bitmap_zero(size_t *dst, size_t nbits)
// {
// 	size_t len = BITS_TO_WORDS(nbits) * sizeof(size_t);

// 	if (SMALL_CONST_NBITS(nbits))
// 		*dst = 0;
// 	else
// 		__memset(dst, 0, len);
// }

// SAI void bitmap_fill(size_t *dst, size_t nbits)
// {
// 	size_t len = BITS_TO_WORDS(nbits) * sizeof(size_t);

// 	if (SMALL_CONST_NBITS(nbits))
// 		*dst = ~0UL;
// 	else
// 		__memset(dst, 0xff, len);
// }

// SAI void bitmap_copy(size_t *dst, const size_t *src,
// 			size_t nbits)
// {
// 	size_t len = BITS_TO_WORDS(nbits) * sizeof(size_t);

// 	if (SMALL_CONST_NBITS(nbits))
// 		*dst = *src;
// 	else
// 		__memcpy(dst, src, len);
// }

// SAI void __bitmap_set(size_t *map, size_t start, ssize_t len)
// {
// 	size_t *p = map + BIT_WORD(start);
// 	const size_t size = start + len;
// 	int bits_to_set = WORD_WIDTH - (start % WORD_WIDTH);
// 	size_t mask_to_set = BITMAP_FIRST_WORD_MASK(start);

// 	while (len - bits_to_set >= 0) {
// 		*p |= mask_to_set;
// 		len -= bits_to_set;
// 		bits_to_set = WORD_WIDTH;
// 		mask_to_set = ~0UL;
// 		p++;
// 	}
// 	if (len) {
// 		mask_to_set &= BITMAP_LAST_WORD_MASK(size);
// 		*p |= mask_to_set;
// 	}
// }

/* clear */

/* and */

/* or */

/* nand */

/* xor */

// SAI void bitmap_set(size_t *map, size_t start,
// 		size_t nbits)
// {
// 	if (__builtin_constant_p(nbits) && nbits == 1)
// 		__set_bit(start, map);
// 	else if (SMALL_CONST_NBITS(start + nbits))
// 		*map |= GENMASK(start + nbits - 1, start);
// 	else if (__builtin_constant_p(start & BITMAP_MEM_MASK) &&
// 		 IS_ALIGNED(start, BITMAP_MEM_ALIGNMENT) &&
// 		 __builtin_constant_p(nbits & BITMAP_MEM_MASK) &&
// 		 IS_ALIGNED(nbits, BITMAP_MEM_ALIGNMENT))
// 		memset((char *)map + start / 8, 0xff, nbits / 8);
// 	else
// 		__bitmap_set(map, start, nbits);
// }

// SAI void bitmap_clear(size_t *map, size_t start,
// 		size_t nbits)
// {
// 	if (__builtin_constant_p(nbits) && nbits == 1)
// 		__clear_bit(start, map);
// 	else if (SMALL_CONST_NBITS(start + nbits))
// 		*map &= ~GENMASK(start + nbits - 1, start);
// 	else if (__builtin_constant_p(start & BITMAP_MEM_MASK) &&
// 		 IS_ALIGNED(start, BITMAP_MEM_ALIGNMENT) &&
// 		 __builtin_constant_p(nbits & BITMAP_MEM_MASK) &&
// 		 IS_ALIGNED(nbits, BITMAP_MEM_ALIGNMENT))
// 		memset((char *)map + start / 8, 0, nbits / 8);
// 	else
// 		__bitmap_clear(map, start, nbits);
// }

// static inline void bitmap_next_set_region(size_t *bitmap,
// 					  size_t *rs, size_t *re,
// 					  size_t end)
// {
// 	*rs = find_next_bit(bitmap, end, *rs);
// 	*re = find_next_zero_bit(bitmap, end, *rs + 1);
// }


